from re import X
from turtle import right
import cv2
from cv2 import threshold
import numpy as np
from matplotlib import pyplot as plt
from detect import RoadDetector
import serial

color ={ 
    "purple": {"lower": np.array([124, 77, 51]), "upper": np.array([177, 255, 255])},

    "brown": {"lower": np.array([0, 96, 0]), "upper": np.array([11, 255, 108])},

    "yellow": {"lower": np.array([21,80,128]),"upper": np.array([50,255,255])},

    "blue": {"lower": np.array([100,43,46]),"upper": np.array([124,255,255])},

    "green": {"lower": np.array([56,46,89]),"upper": np.array([89,255,255])},
    
    "red": {"lower": np.array([0,90,163]),"upper": np.array([10,255,255])},

    "orange" : {"lower": np.array([8,128,117]),"upper": np.array([12,255,160])},
}

rightcnt=1

#树莓派开串口

ser = serial.Serial("/dev/ttyUSB0",115200)

def count_color(frame,min,max):

    S_max = 0

    img=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)

    img=cv2.GaussianBlur(img,(5,5),0)

    color_img=cv2.inRange(img,min,max) #处于min到max阈值之间的img部分被赋予255白色

    color_img=color_img[color_img.shape[0]//2:color_img.shape[0],:]

    kernel=np.ones((3,3),np.uint8)

    color_img=cv2.erode(color_img,kernel,iterations=2) #对白色部分进行腐蚀

    _,cnts, _ = cv2.findContours(color_img.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    for i in range(len(cnts)):

        S = cv2.contourArea(cnts[i])

        if S > S_max:

            S_max = S

    return S_max #最大面积区域


def judge_color(S_max):
    if S_max[2]+S_max[3]>20000 and S_max[2]>10000 and S_max[3]>10000:
        print("turnturnturnturnturnturnturnturnturnturnturnturnturnturnturnturnturnturn")
        #print("S",S_max_green+S_max_red)
        rightcnt=2
        print("rightcnt",rightcnt)
        ser.write('B\r\n'.encode())
        #print("B")

    if S_max[0]>20000 and S_max[3]<15000 and rightcnt == 1:
        print("homehomehomehomehomehomehomehomehomehomehomehomehomehomehomehomehomehome")
        rightcnt=1
        print("rightcnt",rightcnt)
        ser.write('T\r\n'.encode())
        #print("T")

    if S_max[5]>15000 and rightcnt == 2:
        print("rightballrightballrightballrightballrightballrightballrightballrightball")
        rightcnt=1
        print("rightcnt",rightcnt)
        ser.write('P\r\n'.encode())
        #print("P")

    if S_max[4]>7500 and S_max[3]<15000 and rightcnt == 2:
        print("leftballleftballleftballleftballleftballleftballleftballleftballleftball")
        rightcnt=1
        #print("rightcnt",rightcnt)
        ser.write('Z\r\n'.encode())
        #print("P")

    if S_max[1]>25000 and rightcnt == 1:
        print("stairstairstairstairstairstairstairstairstairstairstairstairstairstair")
        ser.write('A\r\n'.encode())


#打开相机

cam = cv2.VideoCapture(0)

#og = open("./log.txt",'w')

while True:

    S_max=np.zeros(6)
    m=0
    c=0

    for i in range(0,5):
        ret,orgimg = cam.read()
        #orgimg=cv2.imread("/home/clown/robocup/test/resource/brown.jpg",cv2.IMREAD_UNCHANGED)
        S_max[0]=count_color(orgimg,color["orange"]["lower"],color["orange"]["upper"])+S_max[0]
        S_max[1]=count_color(orgimg,color["blue"]["lower"],color["blue"]["upper"])+S_max[1]
        S_max[2]=count_color(orgimg,color["red"]["lower"],color["red"]["upper"])+S_max[2]
        S_max[3]=count_color(orgimg,color["green"]["lower"],color["green"]["upper"])+S_max[3]        
        S_max[4]=count_color(orgimg,color["brown"]["lower"],color["brown"]["upper"])+S_max[4]
        S_max[5]=count_color(orgimg,color["purple"]["lower"],color["purple"]["upper"])+S_max[5]
    print("S_max:",S_max)

    judge_color(S_max,orgimg)

    #读串口数据，调试用

    '''recv=0

    sercount=ser.inWaiting()

    if sercount!=0:

        recv=ser.read(sercount)

        print(recv)

    if(recv==b'top'):

        stair_flag=0'''

    detector = RoadDetector([([0,0,80],[255,255,255])],[(69,0,131),(121,113,255)],[(83,94,79),(104,175,207)],7)

    #detector.setStairMode(True)

    Read=ser.read_all()
    print(Read)
    print("rightcnt",rightcnt)
    #图像处理

    img = cv2.resize(orgimg,(orgimg.shape[1]//2,orgimg.shape[0]//2))

    #img = cv2.flip(img,1)

    if rightcnt==2:

        img = img[img.shape[0]//3+90:img.shape[0],0:img.shape[1]]

    else:

        img = img[img.shape[0]//3+90:img.shape[0],:]

    hls = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)

    #巡线处理

    detector.update(hls)

    if True:

        pack = detector.getTrackingData() #pack 包含了拟合直线的斜率（k）和截距（b）以及直线的起始点（pt1）和结束点（pt2）

        #print("update")

        if pack is not None:

            #拟合直线数据

            (k,b,pt1,pt2) = pack

            #print('(',img.shape[0],',',img.shape[1],')')

            pt_mid=((pt1[0]+pt2[0])/2-160)/320 # 

            #print("k",k)

            if k>1.2:

                #pt_mid=0

                k=1.2

            if k<-1.5:

                #pt_mid=0

                k=-1.5

            if True:    

                #ser.write(('M\r\n').encode()

                movex=int(pt_mid*100/2.5)/100

                ro=int((-k*10*100/180+pt_mid*10)*100*1.3)/100
                #print('pt_mid',pt_mid)
                #print('movex',movex)
                #print('ro',ro)

                #拟合直线数据与单片机步态控制的线性关系（调试得出）

                ser.write((str(movex)+' '+str(ro)+' \r\n').encode())

                    #print(pt_mid)

            #print('angle',int((-k*20*100/2/100+pt_mid*25)*100*1.5)/150)

            #print('movex',int(pt_mid*200/1.2)/100)

            #log.write("r:"+str(int((k*10*100/2/100+pt_mid*25)*100*1.5)/100)+"\r\n")


    #显示图像

    detector.showTracking(img)

    cv2.imshow("hls",hls)

    cv2.imshow("test",img)

    cv2.imshow("mask",detector.img_mask)

    #cv2.imshow("origin",orgimg)

    if ord('q') == 0xff&cv2.waitKey(10):

        break

#log.close()

cv2.destroyAllWindows()